#ifndef __DRV_MOTOR_H__
#define __DRV_MOTOR_H__

#include "pid.h"

#define MOTOR_ID 0x205 // ID为1
#define ROUND_LEN 8192
#define REDUCT_RATIO 1.0

#define SPEED_PID_PARAMETER 5, 0.5, 0, 1000, 16000, -16000 // 速度环pid
#define ANGLE_PID_PARAMETER 0, 0, 0, 100, 300, -300        // 角度环pid

// pid控制周期 单位ms
#define PID_CTRL_IN_PERIOD (1)   // 内环运行周期 速度环
#define PID_CTRL_EX_PERIOD (2)   // 外环运行周期 角度环
#define MOTOR_OFFLINE_TIME (500) // 电机离线的阈值,大于这个值则认为电机离线

// 对i的特殊处理
#define ANGLE_PID_I_THREADHOLD 4096
#define SPEED_PID_I_THREADHOLD 20
#define I_CTRL_K (0.2f)

// 6020电机消息发送
#define MOTOR_SEND_ID 0x1FF
#define MOTOR_ID_6020 0x205

#define PI 3.1415926

static float AngleNow; // 当前角度

/*电机闭环方式*/
typedef enum
{
    LOOP_CTRL, // 闭环
    NO_CTRL,   // 无闭环
} Crtl_e;

typedef struct DjiMotor
{
    rt_uint32_t motorID; /*电机反馈报文ID*/

    rt_int32_t speed;
    rt_int32_t current;
    rt_uint8_t temperature;
    rt_int32_t angle;        /*编码器原始角度 0~8191*/
    rt_int32_t old_angle;    /*上一次编码器数值*/
    rt_err_t oldangle_state; /* 记录当前历史数据是否有效，首次使用时历史数据无效 */
    float velocity;          // 前馈速度
    float last_velocity;
    float acceleration; // 前馈加速度
    float extra_angle;  // 记录的圈数之外多出来的角度 单位 °
    rt_int32_t loop;    /*一共所转圈数*/
    float ratio;        /* 填写电机输出轴转动一圈时，编码器数据转过的圈数，以完整的M3508为例，应填写19.0f */

    Crtl_e Angle_CtrlMode; // 记录角度控制模式
    rt_int32_t Round_Len;  // 角度环计算时的整圈长度
    rt_int32_t Set_MAX;    // 角度环计算时的角度数值的最大值
    rt_int32_t Set_MIN;    // 角度环计算时的角度数值的最小值
    rt_int32_t Data_Valid; /* 是否在初始化电机结构体后接收到新报文 */

    rt_int32_t FreshTick; // 记录最后一次数据刷新时刻，用于判断电机是否离线

} DjiMotor_t;

typedef struct
{
    DjiMotor_t dji;
    pid_t ang;
    pid_t spe;
} Motor_t;

//设定值规划的结构体，存储有关设定值规划的参数及状态
typedef struct 
{
    // 用户设定参数
    float target_angle; // 目标位置
    float max_vel;    // 最大速度 
    float max_acc;    // 最大加速度 

    // 规划出的轨迹参数
    float start_angle;  // 起始位置
    float actual_vel; // 实际速度 
    float t_acc;      // 加速段时间
    float t_peace;   //匀速段时间
    float t_dec;      // 减速段时间
    float t_total;    // 总时间

    //规划完成的状态
    float pos_setpoint;//在规划中某次设定的值
    float vel_feedforward;//速度前馈
    float acc_feedforward;//加速度前馈

    // 轨迹执行状态
    float current_time; // 从轨迹开始起的当前时间
    uint8_t status;     // 状态: 0=停止, 1=执行中, 2=完成
}Set_planner;


extern Motor_t motor_data;
#endif